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ABSTRACT 


A sequential Extended Kalman filter routine was developed 
to provide real time estimates of torpedo position and depth 
on the three dimensional underwater tracking range at the 
Naval Torpedo Station, Keyport, Washington. Filter measure- 
ments consisted of acoustic pulse transit times from the tor- 
pedo to a receiving hydrophone array. These measurements, 
теп аге попізпеауү functions of the position and depth coordi- 
nates, were linearized and the filter gains calculated on-line. 
tests were conducted using simulated torpedo trajectories 
that traversed both single and multiple hydrophone arrays. 

It was found that filter performance was dependent on system 
noise and the distance the torpedo was from the hydrophone 
би Position and depth errors ranged between 0 and 10 
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I SINTRODUCTEON 


The Naval Torpedo Station (NAVTORPSTA) Keyport, Washington 
currently operates two three-dimensional (3-D) underwater 
tracking ranges with acoustical capability. The underwater 
tracking range utilizes a sonar transmitter installed in the 
BEornpedo to be tracked. The transmitter is synchronized with 
a master clock.  Timed acoustic pulses are received by bottom 
mounted hydrophone arrays and then relayed via cable to a com- 
eer ae the observation site. The computer calculates the 
positional coordinates of the torpedo and plots its trajectory 
through the water. 

The measured data, which consists of the elapsed time 
from transmission of a pulse until its receipt at the hydro- 
phone array, is corrupted with noise due to the combined ef- 
fects of environmental factors and measurement instruments. 

In order to achieve a more accurate plot of the torpedo 
path, editing and smoothing techniques have been applied to 
the data during post run analyses. 

To improve the real-time capability of the 3-D tracking 
ranges, Kalman filtering theory has been applied to the com- 
puter generated X, Y, and Z positional coordinates but is 
not operational [1]. 

In the near future an updated computer system composed 


of three MODCOMPIV computers will be in operation. The 








majority of the software development requires interfacing 
current tracking programs and other related programs to the 
Mew computer system. 

An opportunity exists for expanding the capability of 
the tracking system by applying a Kalman filter routine which 
Enocesses measurements of the transit times of the acoustic 
pulses and provides an estimate of the position of the 
мса әрцесс іп real time. Prior research in this area 
K revealed that a Kalman filter utilizing a sequential 
estimation approach was desirable. 

ime Іпсепеісопі 15 to develop and test a sequential Kalman 
Beurer tracking algorithm that can be interfaced with the 


Eurrent underwater range system. 





II. DESCRIPTION OF RANGE TRACKING GEOMETRY 


The hydrophone array, consisting of four independent ele- 
ments, defines an orthogonal coordinate system in which transit 
pore measurements are made. As shown in Figure І, four hydro- 
eones X, Y, Z, and C are on four adjacent vertices separated 
by a distance d, along the edge of the cube. The origin of 
the array coordinates is at the center of the cube with the 
orthogonal coordinates parallel to its edge.  Positional 
information is computed from the transit times of a periodic 
Exehronous acoustic signal travelling from the torpedo to 
mie four hydrophones on the array. The range measures the 
meacked torpedo's position every 1.31 seconds to an accuracy 
morse tyolcally wiehin 3 to 30 feet, A more detailed de- 
ореол of the range tracking capability is described in 


№]. 
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ТТТ.  THEORY--EXTENDED KALMAN FILTER 


IHDSURBI--CLAIDLNULDO KALMAN FILTER 
The equations describing the nonlinear relationship 
between transit times T ЛЕ T r- and Әле posicion or 


"E 777 72 
Ее “гаскей torpedo are given by [3] 


Тс = у (сх + 500 + (у + 97 + (2 + 9215, СО, 
т, = фер Пх – 95 + (у + 93 + (а + 9215, (3.2) 
mo E [(x + it + (y Ti + (2 + 5) =} (3.3) 
T, yep [х + 97 + (у + 52 + (2 - 79%, (3.4) 


where d = 30 feet and VEL = 4860, which is approximately the 
average velocity of propagation of sound in water at Dabob 

Bay. Since the transit times were readily available and are 
nonlinear functions of position, these equations can be linear- 
ized and Kalman Filter theory applied using the Extended Kalman 
Filter [2]. This procedure producses a real time measurement, 
КИ leering on the corrupted transit times Tor Tor Ту, апа 


T, without the necessity of converting these times to positions. 


"sound се Шо C in water varies as a function Of temperature, 
salinity, and depth. 


Jet 





Borschercehree-dimensional location problem three position 


Bates (x, y, Z) and two velocity states СТ Ue specify 
target motion. The discrete linear and nonlinear observation 
equations are given by 

x(k +1) = © + x(k) + T+ w(k) (3.5) 
and 

2(К) = h(x(k),k) + v(k) (3.6) 


In these equations ¢ апа T are constant matrices and h is а 


ee eea function of the state variable x. w(x) is plant 


excitation noise and v(k) is measurement noise. The plant 


noise and measurement noise are assumed uncorrelated (white) 


with zero mean. 


That 


and 


with 


15, 
F [w (K) 
F [v (k) 
es des 
О, 
In order 


K 


5, 
G. 
| 
КО 
> 
О» 


т 


БО лор the linear filter eguation (3:0) s 


expanded in a Taylor series about the best estimate of the 


2 








the state at that time and only the first-order terms are 


kept. 


Beuatıon (3.6) gives 


2(К) = H(k) - ЖЕ) т v(k) Cow) 
where 
Н(К) = En (3.7a) 


и 


ЕЕЕ l) 1s the predicted value of the state before the 


th 


k 


and 


The 


and 


by 


measurement. 


есес error vector is defined by 


= x(k) - x(k), 


[x 


a predicted state error vector is defined by 
x(k/k-1) = x(k/k-1) - x(k) 

Covariance of state error matrix is defined by 
P(k) = Elx(k) - x (k)], 


the predicted covariance of state error matrix is given 


P(k/k-1) = E[x(k/k-1) + x7 (k/k-1)] 


The state excitation matrix is given by 


T 
Q(k) = T(K)E[w(k) + w'(k)] - T” (k) 
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and the mea 


R(k) = 


surement noise covariance matrix is 


E[v(k) - v (k)] 


The Kalman filter equations are given by [4,5]: 


P(k+1/k 


G(k) 


БОК) = 
x(k+1/x 
z(k/k-l 


x(k) = 


) = Фр(к/к)Ф + Q(K) 
P(k/k-1)HT(k) [H(k) . р(к/к-1)ні (к) 4 R(k)]" 
[[ — G/ (k)H(k)]P (k/K-1) 


= öx(k/k) 
) = h(x(k/k-1) ,k)) 
х(к/к-1) + G(k) [z(k) - 2(k/k-1)] 


L 


592 


(1959 


в 


Ва) 


8b) 


БОС) 
. 8d) 


. 8e) 


ӨТ) 


The Q matrix serves not only to allow for maneuvering but 


ESO to acc 


ount for any model inaccuracies, that is, any dis- 


crepancies between the true action of the torpedo and its 


@eacacteriz 


prevent the 


а ОИ (15,2), Шас б also Serves 


EO 


gain matrix G(k) from approaching zero by always 


EuSusSHg uncertainty in the predicted covariance of error 


Matrix P(k+1/x) . 
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IV. PROBLEM DEFINITION--TORPEDO TRACKING WITH THE EXTENDED 
KALMAN FILTER 

С СТУБ EQUATIONS 

Figure 2 illustrates the geometry of the states used for 
the filter. For a Constant course, constant-speed torpedo, 
the plant state equations can be described as two second 
order systems, one for X and one for Y. The depth (2) of the 
torpedo is maintained constant and any velocity in the Z di- 
rection is assumed to be random, uncorrelated with zero mean. 
Z is therefore described as a first order system. 


The plant state equations are 


x(k+1) ЕСТ "Ye С) 
Е t 
v. (k+1) SE "Ys 1K) 
E ДЕ 

= | y(k)+Tv (x)+ mE 

ieri) y (k) ше, ) САС ) 
| и Ч 
E ) | К) + n vue | 
у Vy! ЫСТ p (4.1) 
2 (К+1) z (K) +g- (Yz) 
where x(k), and y(k) are the position coordinates at time 


E(k); v (K) and 2 are the X and Y components of velocity. 
T is the time between observations, i.e. t(k+l)-t(k) and z(k) 


ls the depth of the torpedo. 
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Figure 2. Filter Geometry 
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The excitation terns 91 through gs are included to account 
for the random changes in speed, heading, and depth which can 
occur in a maneuvering torpedo. The quantities Y Y 

Vr O 1 and 
E 
|| are the random changes of the torpedo which are assumed 


to be independent, zero mean, rates of change. They have 


variances defined by 


2 2 
S = Ely, ] 
Es ДЕ 
ој E 
9,2 = Elva 2] 
2 
: 2 
о ES 


The values of the standard deviations are taken from typical 


maneuvering parameters for the torpedo; 


| —=— 94 = 22°/sec 
Е 
2 
>= 0. E 360 f5/ Sec 
V 
E 
Са 0. = 


The effect of this excitation is to increase the predicted 
value of the covariance of the state error matrix. 


Writing the equations in state form resuts in 


x(k+1) = x(k) + Tw(xk) (4.2) 
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where 


and 


| 
i. 0 0 
у 
E 2 
Шы D. 0 
0 T 0 
0 0 T 


IESO y) represents the effect on the states of the 

random excitations. Reference 5 has shown that the vector 

may be calculated from the equations relating torpedo X and 
Y velocity to the torpedo heading and а. Тре Х 


F  OcCity is 


which when differentiated gives 


<= -V, 6,516, - У сове, (4.4) 
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where 


Jar 
о 
. 9 КЕ Vy 
Salt = = V. 
3 _х 
cos = — У. 


ПИ upon substitution of the above relationship in equation 


(4.4), 
Vo 
X = -у Ya + y. ШЕ. (4.5) 
Е Ve V+ 
ami larly 
е = 4.6 
Y E v sino, ( ) 
y = “9, 2059. + v sinô, 
and after substitution 
“ү 
y = Voy: + Y . 4 8 
x O, Vi Ve (4.8) 
The depth term is just 
2- Үү» . (4.9) 
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Thus 


Be Ух * 
w(k) = B vo- và Мај (Ya V ЕЕЕ 


сї 
ct 
he 
ct 
A 
< 
ct 
ct 
N 


wnere from the assumption on the y's 


E tero 0 


The excitation covariance matrix is thus found from 


Q = TE [w(k)w(k) Ir. 
Ya 7 4х5 IX) 
Let 
V 
u = (=) с, : арау fo; 
EJ e Yo Ct 


8 
t Ve E 
95 
ДЕ 
Ge = V [320,95 
ху XY. t 
E 


(4 


(4 


(4 


(4 


. 10) 


2112 


51572) 


Ја) 


39) 


else) 


where the states are evaluated at the current state estimates 


E). Substituting these expressions in the Q matrix results 


ШТ 
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2 0 
| У 
Syme tre 
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The excitation matrix serves not only to take into ac- 
count the possibility of maneuvering, but of model inaccuracies 
as well [5]. О 15 also used to prevent the gains of the filter 
from approaching Zero as more and more data is processed, by 
insuring some uncertainty in the predicted state values. 

The observation equations are nonlinear in the states 


ЕШСІ are given by 


о о 


Th) | 1 
C | VEL 
1 2 2 p 
T (k) van (®) -9/2) +(y(k)+d/2) (206) +d/2) ] ° q+ h b) 
z(k) = = (4.15) 
T, (k) m [(x(&)+4/2) ^e (Q9 2/2) ^ (0) 44/2) * * v) 
1 2 2 25 MN 
| 1,09 vgl GO) 98/2) «Gy 0944/2) ^ (2 0) 24/2) "* + VCO) 


2 








m 


Ihe measurement noises, v(k)'s, are assumed to be zero-mean 


and independent with a covariance matrix 


Г. 2 

p$ 7 
| Te 0 0 0 | 
ZO 6 0 iu 

X 
R(x) = (4,16) 
0 0 Жа 0 
Y 


B@emmacghitude of the transit times noise is a function of 
the signal-to-noise ratio at the hydrophone array, which is 


influenced bv several environmental factors. 

Discussions with NAVTORPSTA versonnel indicate a typical 
accuracy of + 10 microseconds and this value is used as Ba 
standard deviation for the simulation runs, 

Equation (3.7a) can be used to give the linearized observa- 


tion matrix. The result is 











To dTa (K) oTo (K) dTa (K) oT (Kk) 
ox 9 V ду ду 02 
x ү 
ту (К) от (К) eu CR) эт, (к) IT (K) 
ох DR ду ev, 02 
ВИС) = (4.17) 
oT, (x) эт, (к) эт, (к) oT (K) oT (K) 
OX QV ду 9 V g 
x Y 
oT, (K) oT, (K) aT, (K) oT, (K) оТ, (К) 
ох ОМ ду оу 92 
= x Y as 
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when the derivatives are taken and evaluated at the pre- 


dicted state values x(k/k-1) = x'(k) the result is 


Ye A A ES ed/2 
x' (X) -d/2 Y (Ta? z'(k)+d/2 
DEN2 О0О 58-7 9 mn 
Bpv-- (4.18) 
x! (k) +d/2 y'(k)-d/2 2' (к) +4/2 
DEN3 и DEN3 ог ин 
AS A IE A 210-972 
DEN4 = DEN ` |. DENA . 
where 
DENI = RM ug eM D ns 
DEN2 — [(x'(k)-3/2) ?« (y' () 8/2) ^» (z* (k) «a/2) 2]? 
DEN3 = [(x' (k)+d/2) +(у' (К) -4/2) 2+(2' (к) +а/2) 2] 
рЕМА4 = [(х' (к) +4/2) + (у' (к) +4/2) +(2' (к) -4/2) 71% 


The 0 matrix, O matrix, R matrix, and H matrix are then 


used in the Kalman filter equations (3.8). 


В. THE SEQUENTIAL EXTENDED KALMAN FILTER 

The Kalman filter equations described in the previous 
section represent a system that is characterized by a one- 
step estimation and prediction for each set of four transit 
time observables (Те; ту, Ту, апа ТА). This one step process 
exhibits the following salient features: 
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1. The four transit times received during each 1.31 
second measurement interval (time slot) are processed simul- 
taneously by the filter equations. Updated covariance of 
error and state estimates are therefore obtained only once 
men cach time slot, 

2. A major part of the time period for the estimation 
calculation is spent in the gain equation where the inversion 
of a 4 x 4 matrix is required. 

3. The Extended Kalman filter is based on a linearization 
about the predicted value of a period t. го ttl. Unaccept- 
ably large or erroneous transit time measurements will cause 
the filter to ignore the update information for the entire 
time slot. The predicted estimate will become the new esti- 
mate for the time slot and the interval extended. 

If one assumes that each of the four hydrophones in the 
Штау are statistically independent of one another, a se- 
quential approach can be taken to process each of the four 
transit times separately. Thus, each observable will be 
processed in sequence, and the result of processing one 
measurement is used in the following computation to process 
the next measurement until all four observables have been 
used. 

The advantages of the sequential approach over the tra- 
ditional one-step estimation and prediction are: 

l. Since the four transit times are independent and 


processed sequentially, the covariance of error matrix ana 
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the state vector are updated four times during each time 
slot. Thus more accurate estimates of the filter states 
are achieved. 

2. Modification of the filter equations for the sequen- 
tial approach circumvented the matrix inversion in the gain 
Eusatron. Therefore filter cycle time is reduced substantially. 

3. An invalid transit time measurement will result in 
ше ітісег ignoring the update information for that particular 
Measurement only. For example, if only three of the four 
transit times are acceptable, the filter would be sequentially 
Mecacea Ehree times during that particular time slot. Thus, 
the filter is less sensitive to erroneous transit time 
measurements. 

To utilize the sequential Extended Kalman filter, equations 
(3.8) must be modified. Calculations are performed on each of 
the four independent transit times in the following order: Tos 
Ту, E and T; for each 1.31 second time slot. “he estimate 
of the states x(k/k), based on one transit time measurement 
are used as the prediction x(x/k-1) for the calculations on 
the next measurement. Thus for the first time measurement To 
ЕШ спе first row of the linearizing H matrix is calculated. 

Next the first gain column corresponding to the first 


aime measurement T, is calculated by 


C 
РИК туне 
С " irow "mS 
icol T 
Hs row! LIE von Fo E 
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where i = l to 4 corresponding to the four measured transit 
times. Thus, the first row of the H matrix is used to calcu- 
late the first column of the gain matrix with both correspond- 
ing to the first measured time Тс. 

Next, an estimate of the particular observation time is 
calculated using equation (3.3e) evaluated at the predicted 
Brite x(Kk/k-1). 

The difference between observed transit time and the 


eotimatead transit times forms the residual which is used in 


the estimate equation 


x, = x(k/k-1) + G [Residual] (4.20) 


J o l 


This equation gives an estimate of the states based on one 
measurement. 
Nes he covariance Of error is calculated based on one 


measurement by 


. = - С. li І 4.21 
a 1 Sicol ро a 
where 
Ww identity matrix 
S = the covariance matrix calculated from the pre- 
vious transit time measurement or if i=l, 


the prediction P(k/k-1). 
After the first iteration, X, becomes x(k/k-1) and P, becomes 
P(k/k-1) for the second iteration which calculates the estimate 


of the states based on the second measurement Ту. 
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After four iterations (k = 4), X4 becomes the estimate for 
the time slot x(k/k) and P, becomes the covariance of error 
К/К). 

The predictions for the next time slot are calculated 


ШЕШІП е сна лопз (3,ба) апа (3.84). This process is repeated 


for each time slot. 
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IV. TESTING AND SIMULATION 


A. DESCRIPTION 

The sequential Extended Kalman filter routine is tested 
using simulated torpedo tracks generated by the IBM-360 com- 
puter. A variety of track scenarios are produced to test the 
filter performance during single array tracking. 

The first series of tests used constant course and speed 
tracks which transitioned multiple array quadrants including: 

crossing north of the array 
2. Crossing above the array 
3. crossing diagonally through the array 

The next series of tests adds a number of typical torpedo 
maneuvers to the constant course and speed tracks. These ma- 
Meyers consisted of 10 and 20 */sec turns with G forces 
ranging from 1/4-G to 1-G. 

For the above tests filter initialization errors range 
from 0 to 40 feet/sec for X and Y velocity, and 0 to 60 feet 
for X and Y position. The Z depth is maintained constant at 
300 feet which is approximately the average depth of water 
аы раБоЬ Бау. Zero mean Gaussian noise is added to corrupt 
the observed transit times for all runs. 

The last series of tests demonstrates the ability of 
pae filter to track through the areas of multiple arrays. 

Before the above series of tests can be conducted, it 


is necessary to add operational features to the software which 
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provide the following capability: 
1. detect erroneous transit time measurements 
2. minimize position error during high dynamic maneuvers 
3. track through multiple arrays 

A more detailed explanation of these features is described 


below. 


B. EDITING ERRONEOUS TIME MEASUREMENTS 

The operation of the filter may be adversely affected by 
erroneous transit time measurements. Large errors in the 
transit time measurements will produce errors in the state 
Meetor estimate and the linearizing H matrix. This may cause 
the filter to become unstable and exhibit divergent character- 
istics. In some extreme cases catastrophic failure may result. 
Measurement errors can occur because of many factors including 
EnMecror їп the transit time of the acoustic pulse primarily 
we othe receipt of multi path signals from previous time 
slots that have reflected off the surface, bottom, or different 
density layers in the water. 

To guard against catastrophic failures a three-sigma gate 
Meingstche covariance of measurement noise (В) and the covari- 
шее Of estimation error (P(k/k)) is implemented. For each 
calculation of a state estimate (x(k/k)), the largest posi- 
B. ovyariance of error is used, either x, у, ог z, and 
converted to time in seconds using the average velocity of 


zn wu "water for Dabob bay, 4860 ft/sec. The gate then is 
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written for each time measurement i = 1 to 4: 


a een 
4860 


where j = 1, 3, 5. The gate expands or decreases depending 
on the confidence level of the position estimate and the 
transit time. If ZDIFF which is the difference between the 
actual transit time received and the predicted transit time 
to a particular hydrophone exceeds the gate, the measurement 
[encoOnsidered unacceptable and the filter gain is set to 
zero causing the filter to ignore the data and take the pre- 


diction of the states as the estimate 
x(k/k) = x(k/k-1) 


An invalid time measurement zeros only the gain column for 
erar ticular hydrophone causing only that hydrophone's 


data to be ignored. 


с. BOUNDING RESIDUAL BIAS ERROR 

As previously mentioned, the torpedo dynamics are modeled 
НЕ Пе filter s excitation covariance matrix Q. 

During preliminary tests it becomes evident that torpedo 
turn rates between 5 and 20°/sec yield position errors ranging 
ласере штеавеју 10 to 40 ft. Increasing the standard devi- 
ation parameters (сё B o for the adaptive O matrix helps 
to a certain extent, however as expected a point is reached 
where the noise starts to become the dominant error source. 


There are two major reasons why the filter develops errors 


during turns: 
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1. The filter's predicted state estimates are computed 
assuming a constant velocity over a 1.31 second sampling 
interval. During a turn the velocity vector of the torpedo 
is continuously rotating, therefore the filter's predicted 
Pelmate will be in error. A large magnitude error will re- 
quire more than one time slot to remove. A faster sampling 
rate would reduce the error and may alleviate the problem; 
however, the Dabob tracking range system has been designed 
for a 1.31 second sampling rate and cannot be changed. 

2. The adaptive Q matrix assumes that the torpedo ac- 
eak rations are normally distributed and uncorrelated. Torpedo 
tracking maneuvers do not fit these assumptions. 

To minimize position errors during maneuvers a procedure 
has been developed in order that repeated estimates of the 
state vector are obtained during a single sampling period 
MENS] second time slot) until the transit time residual error 
falls below a preset threshold. 

In Section IV it was stated that during each time slot 
Afina, Covariance, and state estimate equations (4.19- 
4.21) were iteratively solved for each of the four sequential 
transit times Тс, Ту, Ту! апа Tz. Subsequently additional 
БЕ те мас incorporated to calculate the estimated transit 
time to each of the four hydrophones based on the last up- 
dated state vector. Four time residuals are then computed 
by subtracting the estimated time to each hydrophone from the 


observed transit time. The average of the absolute value of 


ЗА 





ен ште residuals is then formed. If the average residual 
error exceeds the preset threshold in the software, which is 
indicative of large error in the state vector, O is calculated 
and added to the last updated covariance of error matrix P. 
The filter then reiterates the gain, covariance, and state 
estimate equations for the same time slot. ТЕ the new state 
vector generates an average residual error still exceeding the 
threshold value, then the process is repeated. This procedure 
continues until the average residual error falls below the 
preset threshold at which time an acceptable state vector 
estimate has been obtained for the time slot. Once this oc- 
curs the prediction equations (3.8a) and (3.8d) are solved 

and the filter proceeds to the next time slot and commences 
processing new transit time measurements. As will be shown 
later the filter did successfully bound the position error 
during turning maneuvers. For these tests the threshold con- 
stant is set at 10 microseconds, which is the value of RMS 


measurement noise. 


C. MULTIPLE ARRAY TRACKING 

Matta ll ests were performed on tracks im the area o£ 
one array. In order to more closely simulate a typical run 
on the range, a scheme was designed to track the torpedo 
through multiple arrays [2]. 

First, a coordinate system is defined as shown in Figure 


3. The center of the coordinate system is geographically 
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Figure 3 


25 





near the entrance to Dabob bay in the simulation. Array num- 
er 6 is the closest array to the coordinate center. In the 
simulation array 1 15 ағ 36,000 feet from coordinate center 
and array 6 is 6,000 feet. The C hydrophone is assumed to be 
Ehe axis location of each array. Then each X position for the 


EE drophone in each array is X, + 30, each Y position for the 


C 
Y hydrophone is Ус + 30 and each Z position for the Z hydro- 
phone is Zo + 30. These 72 positions, an XYZ position for 


each of 4 hydrophones in 6 arrays, are placed into a 6 x 12 
matrix HYDRO and referenced throughout the routine. The 
torpedo position is referenced to a central level rectangular 


coordinate system. The nonlinear observation equations become 


1 | ы O 2_% 
TG Ck) p LX uL) cen +(z'(k)-2,.) E 

l ; 2 2 ; 2,% 
т, (к) гок) ех, у) 7+ (у? (к) Уу) +(2' 09-2, 0] 

6 СЕ) — = 

ү ! 2 t 2 1 2 = 
T,(k) TELLO (k)-X,,) +(y Ск) у) +(z (k)-2,,) ] 

1 t 2 1 2 1 1 2 % 
7,0%) ы SS S СЕ) a 15] 


where TR), ТК), ТК), and т, (К) are the transit times to 
Eres rhe four hydrophones and X' (k), Y¥'(k), and Z'(K) are 
the filter estimates x'(k/k) evaluated at the predicted 
positions x (k/k-1). The subscripted variables X, Y, and 2 


are the coordinates of a particular array being used. 
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The decision parameter used to determine the switching 
Beem array tO array is a straight handoff. If the predicted 
x position is greater than 3,000 feet from the array in use, 
then an index (I8) is incremented and the next row of HYDRO 
is implemented. This placed into the routine the X, Y, and 2 
positions of the hydrophones in the next array. The handoff 
can easily be utilized in real range operations, as the 
transit times from adjacent arrays are present at the compu- 
Sector a particular time slot. For simulation, it is as- 
sumed that in all the arrays each axis pointed in the same 
direction. In actual range operations each array is tilted 
ADO ca the X and Y axis. Since the true transit times 
are derived in a tilted coordinate frame, the filter's esti- 
mate of transit time must also be calculated in a tilted 
coordinate frame. The tilt angle measurements along with 
the level rectangular coordinates of the array with respect 
to the central rectangular coordinate system can be input 
Bros the matrix HYDRO to rotate the coordinates of each 


Brärophone in the array. 
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VI. TEST RESULTS AND DISCUSSION 


A. SERIES ONE--STRAIGHT RUNS 

This series of tests includes constant speed and heading 
m eS. Initial position and velocity errors are set to 
zero. Measurement noise is added to all runs. Torpedo ve- 
ШЕГУ ranged from 20 to 55 knots. Figure 4 shows the true 
Eeeyectory of the torpedo in the horizontal X-Y plane. The 
En er Of the hydrophone array is located at positional co- 
s nates (0,0). Figure 5 describes the filter's RMS estimate 
ENMEDoSsitrion error as a function of sampling time. The RMS 
estimates are obtained by taking the square-root of the ap- 
propriate diagonal terms of the covariance matrix. It should 
be noted that the propagation of RMS estimates are a function 
of the position of the torpedo with respect to the hydrophone 
array. As shows in Figure 4, if Y = 0, the torpedo path is 
inbound toward the array along the "X" axis. Upon examining 
КЕ те 5, it is evident that the RMS estimate of X position 
error or the uncertainty in the filter's X position estimate 
Meme and relatively constant throughout the entire tra- 
соку. The uncertainty in the filter's Y position estimate 
decreases as the torpedo approaches the array, becomes a mini- 
nM at the closest point of approach (CPA), and increases as 
the torpedo moves outbound away from the array. This behavior 


in the filter's RMS estimates can be explained by recognizing 
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Bast the cluster of four hydrophones in the array actually 
describe a position fixing system. The torpedo position is 
Barca Dy the intersection of circular lines of position 
(range arcs) originating from each hydrophone on the array. 

In general, the greater distance the torpedo is located from 
the array, the shallower the range arc intersections become 
and the more uncertainty exists in the actual torpedo posi- 
tion. For the case where the torpedo is moving along the X 
axis, the uncertainty in position exists only in the Y 
direction. If the torpedo's X distance from the array 

becomes infinite, the range arcs from each hydrophone appear 
to originate from a single point source. The position fixing 
system then reduces to a single dimension ranging system along 
the X axis with the uncertainty in the Y position estimate 
becoming infinite. If the torpedo approaches the array along 
the Y axis (X = 0), the filter's RMS estimate of Y position 
error will be relatively small, and as the torpedo's Y distance 
ош пе array becomes infinite, the filter's uncertainty in 
the X position estimate becomes infinite. 

In order to keep RMS position error estimates within 
allowable tolerance the Dabob range tracking system requires 
Enestorpedo remain within a 3,000 foot radius of the center of 
the hydrophone array. 

Figures 6 and 7 depict the error in both position and 
denso: the straight run trajectory along the X axis. 


The position errors are computed by subtracting the filter 
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position estimate, x(k/k), from the computer generated true 
position for each time slot. From Figure 6, it is observed 
that the Y position error magnitude, which is caused by 
system noise,decreases on the inbound leg and increases on 
the outbound leg. Since the torpedo path originated on the 
Euer tracking perimeter (X = 3,000 feet), the noise in the 
Y channel will be amplified due to the large uncertainty in 
the Y position estimate. The magnitude of noise error in the 
Y direction also decreases as the torpedo approaches the ar- 
ray and increases as the torpedo moves outbound. The steady 
state X and Y position errors ranged between -11 and +4 feet 
throughout the trajectory. 

Figure 8 describes a straight inbound torpedo path that 
passes north of the array at a constant Y distance of 2121 
Beet. Torpedo velocity is maintained at 20 knots. A compari- 
son of the RMS x estimates of this track with the preceding 
one, Figures 5 and 9, reveal that both estimates are relatively 
constant, however the X estimate in Figure 9 is significantly 
larger. Since the torpedo path is parallel to the X axis, a 
larger uncertainty in the X position estimate will result. 

EN omparison of the RMS Y position error estimates reveals 
identical propagation characteristics. This is due to the 
symmetry in the east-west direction of the two trajectories. 

Figures 10 and 11 show that the steady state position 


and depth magnitude errors are less than 10 feet. The noise 
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ШШШ Сес €rror in the X position is due to the uncertainty in 
the filter X position estimate. 

A torpedo path which is inbound along a 45° diagonal to 
Mme array is depicted in Figure 12. For this run torpedo 
velocity is constant at 28 knots. Figure 13 shows that the 
filter's X and Y RMS estimates of position error are approxi- 
mately equal. Likewise Figure 14 shows that X and Y position 
error propagation is also similar. These results are consis- 
tent since the X and Y position magnitudes of the torpedo 
trajectory are identical. At the point of closest approach 
(Ж - 0, Y = 0) the uncertainty in both the X and Y positions 
becomes a minimum and increases as the torpedo moves past 
the array. Steady state position and depth errors range 


between -4 and +6 feet, 


B. SERIES TWO--MANEUVERING RUNS 

The results of the straight run analyses discussed in the 
previous subsection show that the propagation of the filter's 
estimate of RMS position error is dependent on the path of 
the torpedo with respect to the hydrophone array. Тіп Еш 
erros in X and Y position, and Z depth are also a function of 
the torpedo trajectory. In this series of tests maneuvers 
are added to the three straight path trajectories. These 
maneuvers consist of 360° turns at turn rates of 10 and 20°/sec. 
G forces range between 1/4-G and 1-G. G forces are changed 


using the same turn rate by varying velocity. A typical test 
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run on the tracking range at Dabob would include these type 
maneuvers. 

Figure l6 describes an inbound torpedo path which main- 
tains a Y distance from the array between -200 and +600 feet. 
Initial position and velocity are 30 and 20 feet in X and Y 
position and 30 and -20 feet/second in X and Y velocity. 
Torpedo velocity is maintained constant at 20 knots through- 
out the trajectory. Four 360? 1/4-G turns are executed during 
Bye run. Figures 17 and 18 describe the position and depth 
errors. Within approximately three time slots the filter 
achieved steady state performance. Upon observing the error 
propagation it is apparent that the filter is insensitive to 
еп сасе maneuvers, Negligible degradation in position and 
depth is observed. Thus the error bound routine discussed in 
the previous section successfully bounds position and depth 
errors during turns. As expected the position errors decrease 
as the torpedo approaches the array and increase as the torpedo 
proceeds outbound from the array. Since position and depth 
r Oro exhibit approximately equal oscillations about zero 
indicates that the system noise is the dominant error source 
што the filter, Bias errors on the time residuals due 
to maneuvers are negligible. Steady state errors in position 
and depth ranged between -l and +5 feet throughout the run. 

The next test describes an inbound torpedo trajectory, 
Figure 19, which passes north of the hydrophone array. The 


torpedo maintained a Y distance from the array between 160600 
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and 2400 feet. Four 10°/sec 1/4-G turns are executed during 
Еб test. Filter initialization errors are 20 and 11 feet 
in X and Y position and 20 feet/sec in both X and Y velocity. 
The error in position and depth is described in Figures 20 
and 21. The X position error demonstrates larger deviations 
than the Y position error. As noted during the straight runs, 
ms 15 аџце to the difference in the filter's X and Y RMS 
eeen error estimates for this type trajectory. The oscil- 
latory characteristics of the position error indicate again 
Ши nose 15 the dominant error source and the filter is 
prdgckeng the torpedo through the maneuvers. Position errors 
range between -8 and +8 feet with one X channel error reach- 
Пише maximum Of -15 feet, which occurred at time slot 140. 
The exact cause of the -15 foot error is unknown but may 

have been caused by an anomaly in the IBM-360 subroutine 
SNORM which generates the zero mean Gaussian noise for the 
ШИ = Errors in torpedo depth, Figure 21, are consistent 
end range between -2 and +5 feet. 

For the next test the torpedo exhibits an inbound Ега- 
jectory along a 45° diagonal with respect to the array. 
que 22 shows that four 10°/sec 1/4-G turns are executed 
on this run: 2 on the inbound leg and 2 on the outbound leg. 
anneer in itialization errors are 60 feet in both Х апа Y 
position and 30 feet in X and Y velocity. Steady state con- 
ditions are achieved in approximately four time slots. As 


expected for this type trajectory, the X and Y position 
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errors portray similar propagation characteristics, Figure 23. 
This similarity can be explained by the equivalent X andY RMS 
error estimates generated by the filter for a 45% diagonally 
inbound trajectory. Position and depth errors, Figure 24, rnged 
ranged between -7 and +10 feet, 

The preceeding tests discussed filter performance for 
Seraicht runs with 10°/sec 1/4-G turns. Using the same basic 
Eprpedo trajectories as above, similar tests are performed with 
ШИ seccturn rates at 1/2-G. Figures 25 through 33 describe 
the torpedo paths and the corresponding position and depth 
БЕКЕТ ргорасастоп plots for each run. Filter initialization 
errors ranged between 10 and 40 feet in position and 10 and 
иШ гесе in velocity. Torpedo velocity is maintained constant 
at 55 knots. An analysis of the position and depth plots 
reveals that filter performance is similar to that described 
in the previous tests. In Figures 26 and 32 the larger errors 
meürred after time slot 135 are due to the torpedo trajectory 
s coding the radial tracking range limit of 3,000 feet. This 
is more clearly described in the torpedo trajectory curves, 
meres 25 and 31. The additional 1/4-G acceleration during 
ЭШ ИО ООШ did not produce visible degradation in the filter's 
position and depth estimates. 

The next set of maneuvering tests increased the turn rate 
to 20°/sec with G forces at 1/2-G. The same basic inbound and 
Outbound straight runs trajectories are used. Initialization 


errors ranged between 20 and 40 feet in position and -10 anc 
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and 35 feet/second for velocity. Figure 34 describes a torpedo 
inbound trajectory which maintains a Y distance from the array 
between -275 and 500 feet. Four turns are executed on this 
run, Тһе X and Y position errors reveal essentially the same 
characteristics described before. In general the position and 
depth errors decreased as the torpedo approaches the acras, 
become a minimum at the closest point of approach, and increase 
as the torpedo moves outbound past the array. The higher turn 
rates experienced on this test do not degrade the X and Y posi- 
Шол ескоке. Figures 35 and 36 show that position and depth 
errors ranged between -l and +5 feet. 

Figure 37 depicts a torpedo path which maintains a Y dis- 
tance from the array between 1760 and 2760 feet. Three 20?/sec 
CG turns are performed during this run. Figures 38 and 39 
describe the position and depth errors which range between -7 
and +9 feet during steady state. 

вле аладопајју inbound trajectory with four 20°/sec 1/2-G 
turns is displayed in Figure 40. Position and depeth errors 
for this run, Figures 41 and 42, ranged between +10 and -10 
feet. Again note the similarity in the position error behavior 
for each channel for this type trajectory. The larger errors 
in position after time slot 125 are due to the filter's RMS 
position error estimates increasing as the torpedo approaches 
L o] tracking limit of the hydrophone array. 

ен ==- set Of maneuvering tests increased the G force 


Ang the 20°/sec turns from 1/2-G to l-G. The three basic 
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inbound-outbound trajectories are again repeated. в: чес 
43 through 51 describe the torpedo trajectories and correspond- 
ing position and depth errors. Overall performance indicates 
that during steady state conditions the magnitude of position 
and depth errors are below 10 feet. 

Due to the presence of system noise position errors 
reached a maximum of 14 feet, when the torpedo trajectory 
exceeded the radial tracking limit of the hydrophone array. 
As was also observed in the preceding tests, errors induced 
by the turn rate appeared to have a negligible effect on fil- 


про кеосктапсе. 


С. SERIES THREE--MULTIPLE ARRAY TRACKING 

The purpose of the last series of tests is to functionally 
demonstrate that the filter is able to track the torpedo 
through multiple arrays using the simulated tracking range 
бс шр апа handoff scheme described in the previous section. 
Pine a2 deprcts a straight run trajectory which traverses 
Messi hydrophone arrays. The position and depth errors 
are described in Figures 53 and 54. 

Note that the handoff from array to array was accomplished 
fee negligible effect on position and depth errors. The mag- 
nitude of the errors compare well with those observed on the 


Single array straight run tracking. 
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VII. CONCLUSIONS 


The sequential Extended Kalman filter satısfactorily pro- 
vided real time estimates of torpedo position and depth. The 
magnitude of steady state position and depth errors ranged 
between 0 and 10 feet for torpedo tracks within the specified 
meatal Cracking range. 

The iterative error bound routine furnished the capability 
to estimate torpedo position and depth during turning maneu- 
vers up to 1-G. Errors generated during maneuvers are compa- 
ШЕ ШӘ tO those observed during straight runs. 

The filter's RMS estimates of position and depth errors 
are dependent on system noise and the distance the torpedo 
веком the center of the hydrophone array. In general larger 
Боска are encountered at more remote distances from the 
Bodrophone array. 

The simulated trajectories were performed using constant 
Bun ment Noise sequences, Since the filtering problem is 
non-linear the error, in the filter's state estimate vector is 
a function of the statistical properties of the noise. Future 
tests should include a Monte Carlo simulation where filter 
performance is investigated for an ensemble of measurement 
noise sequences. 

Additional tests should also include evaluating filter 


performance using trajectories generated from actual torpedo 
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runs on the Dabob test range. These tests would verify the 
adequacy of the noise model in the filter and the ability of 


the software to edit erroneous transit time measurements. 
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Figure 24. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Run in the Area of a Single Array 
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Figure 33. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 35. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 41. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 42. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 44. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 45. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 48. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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APPENDIX A 


PROGRAM DESCRIPTION AND FEATURES 


The sequential Extended Kalman Filter routine used for 
torpedo tracking is modularized for ease of implementation. 
The program is general in nature and many of the parameters 
of the filter are variable including: 

a. The number of states in the filter--N 

b. The number of random forcing functions--M 

с. The number of measurements--JS 

d. Number of time slots--JTIME 
The constant matrices PHI, R, COVW, and GAMMA are initialized 
in tne beginning of the program using data statements. The 
асе (с іпісізіітей місп P(1/0) ana X(1/0) (initial covari- 
ance of estimation error and states) using subroutine INIT. 
The first estimate is at time 1 and continues until ITIME = 
ШЕЕ Е | rue measurement times (ZI) are computed using either 
subroutines TRAJEC or TRAJC3, depending on whether single 
array or multiple array tracking is implemented. Either sub- 


х" Ту! Т.) 


routine will compute four measurement times (Те, Т 
for each time slot. The measurement times are corrupted by 
zero-mean, white Gaussian noise using the IBM-360 subroutine 
SNORM. For each of the four time measurements the corresponding 


Donsorzchezsismearızing H matrix is calculated using either 


subroutine CHROW or CHROW3, depending on whether single array 
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or multiple array tracking is used. The corresponding gain 
matrix column GI is then found. These row and column values 
are utilized in forming the covariance of estimation error, 

PI, for that particular time measurement. Next the estimate 

of the observation time ZHAT from that particular hydrophone 

is formed using the subroutine CZHAT or CZHAT3, depending on 
whether single or multiple array tracking is implemented. The 
residual ZDIFF(I) - ZIC(I) - ZHAT, is then calcualated. Finally 
the estimate of the states XI based on one time measurement is 
calculated and the process is repeated for the next measure- 
ment. After the four iterations, four transit time measures 
are formed using the last estimate of the states, X4, in either 
ENerLocUNtIDncesScZHAT or CZHAT3. The corresponding four transit 
time residuals are then calculated. The average of the abso- 
lute value of the time residuals is formed (ZDIFAV) and com- 
pared to a preset threshold value in the software. If ZDIFAV 
is above the threshold, subroutine QFIND is used to calcuate 

a new Q which is then added to the last covariance of error 
estimate, P4. The software then reprocesses the transit time 
measurements for the same time slot. This procedure continues 
until the ZDIFAV measurement is equal or below the threshold 
value, at which time the last estimate of the state vector 

and covariance matrix become the XKK and PKK values for the 
time slot. Finally, the predictions XKKMI and PKKMI are formed 
before the process is repeated for the next time slot.  PTLOP 
is used to generate line printer plots and PLOTG is used to 


generate VERSATEC plots. 99 








A. PROGRAM SUBROUTINES 

A brief description of the subroutines that are used 
with the filter are described below: 

1. TRAJEC--This subroutine develops the torpedo tra- 
Ee Ory which is used as truth data for the filter. The 
subroutine generates constant velocity and heading tracks. 
turns of any desired turn rate and G force can be performed 
пишеш Етпе атопа the torpedo track. The subroutine outputs 
Ecl cr ен сүмес, ЛЕС), ап” Еһе X, Y, 2 positions, TD(1I), 
Ее torpedortor each time slot. The subroutine is used 
during single array tracking. 

2. TRAJC3--This subroutine performs the same function 
as TRAJEC but is used only during multiple array tracking. 

3. INIT--This subroutine generates the initial state 
vector (X0/-1) and initial covariance matrix (P 0/-1). 

4.  CHROW--This subroutine computes the appropriate row 
Кле Tinearizing H matrix. Each row corresponds to one 
of the four transit time measurements, Tor Tor To; T,- This 
ӘП ЕНЕСІНЕ is used during single state array tracking. 

5. CHROW3--THis subroutine performs the same functions 
as CHROW and used during multiple array tracking. 

6. CZHAT--This subroutine computes the estimated transit 
times for the filter. Four time estimates, ZHAT, are calcu- 
lated corresponding to each of the four true transit times 
ZI(IJ). This subroutine is used during single array tracking. 

7.  CZHAT3--Subroutine performs same functions as CZHAT 
however it is used only during multiple array tracking. 
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8. OFIND--This subroutine develops an adative O matrix 
used a function of the torpedo velocity. Three input 
variables defined in a data statement at the beginning of the 
program can be adjusted: 

aa. SIGACC--Maximum expected horizontal acceleration 
of the torpedo. 

bb. SIGDIV--Maximum expected change in vertical 
velocity. 

cc. SIGCC--Maximum expected turn rate of the torpedo 
in the horizontal plane. 

The values listed in the program were used and kept 
constant during the simulation tests. If the user desires 
ЕЕ ле адартл“е O subroutine software code is provided 
at the beginning of the program to calculate a constant Q 
matrix. 

9. SNORM--This is an IBM-360 subroutine contained in the 
IMSL library. The routine generates zero mean white Gaussian 
noise with an RMS value normalized to 1. The main program 
scales the noise and adds it to the transit times measurements. 

10. PLOTP--This is an IBM-360 subroutine used to generate 
Пен пе pinte: plots. Information on this subroutine сап 
be obtained from the IMSL library. 

ll.  PLOTG--This is an IBM-360 subroutine used to generate 
оС РОЗ. Information on this subroutine Can be 


obtained from the IMSL library. 


TOL 





p UIPDIIY PROGRAMS 


These subroutines were designed to be used for repetitive 


matrix and vector manipulations: 


Ne 


Zu 


PROD--multiplying two matrices 
MMULT--multiplying a matrix and a vector 
VMULT--multiplying two vectors 
TRANS--transposing a matrix 


ADD--addina two matrices 
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